#include "tof.hpp"
int main()
{


//    depth2ply("/home/u/thetaj7/6d.png", "/home/u/thetaj7/6d.ply");

    for(int i=0;i<150;i++)
    {
        std::ostringstream oss;
        oss<<"/home/u/thetaj/"<<i<<".bin";
//                        DecodeBin(oss.str());
    }

    std::cout<<172*224<<"\n";

        cv::Mat dd=cv::imread("/home/u/thetaj7/6d.png", cv::IMREAD_UNCHANGED);
//    cv::Mat dd=cv::imread("/home/u/thetaj4/61d.png", cv::IMREAD_UNCHANGED);
    std::cout<<dd.elemSize()<<"\n";

    cv::Mat K = (cv::Mat_<double>(3, 3) << 217.869247, 0, 110.485161, 0, 217.869247, 84.037331, 0, 0, 1);
    cv::Mat D = (cv::Mat_<double>(5, 1) << -0.136084, -1.520149, -0.000889, 0.000396, 3.170387);

    std::vector< std::vector<cv::Point> > pts(1, std::vector<cv::Point>());

    pts[0].push_back(cv::Point(14,31));
    pts[0].push_back(cv::Point(152,30));
    pts[0].push_back(cv::Point(150,150));
    pts[0].push_back(cv::Point(18,149));

    cv::Mat mask;
    PolygonMask(mask, dd.size(), pts[0]);

#if 1
    cv::Vec4f table=CalPlane(dd, K, D, mask);
#else
    cv::Vec4f table(0.0680254, -0.0282133, -0.997285, 548.699);

    std::cout<<MeasureObject(dd, K, D, mask, table, 10, 50)<<"\n";

    std::cout<<MeasureBox(dd, K, D, mask, table, 10, 50)<<"\n";
#endif

    return 0;
}

